#include "NearKickBehaviour.h"

/*
 * Spravanie kopne loptu na zadany bod.
 *
 * @param	ciel	bod, na ktory sa ma lopta kopnut
 */
void NearKickBehaviour::Behave(const Point& ciel)
{
	SKILLS_DBGN("%i: ->near kicking to position", world->state->GetSimTime());
	SKILLS_DBGN("%i: ->pos (%3.2f, %3.2f)", world->state->GetSimTime(), ciel.GetX(), ciel.GetY());

	// Ak je lopta dostatocne blizko
	if(DATA_STORAGE.ballDistance < (world->me->type->player_size + world->me->type->kickable_margin + world->params->server->ball_size))
	{
		if(DATA_STORAGE.ballDistance < (world->me->type->player_size + world->params->server->ball_size))
		{
			// Nemoze byt tak blizko
			DATA_STORAGE.ballDistance = world->me->type->player_size + world->params->server->ball_size;
		}

		float dir_diff = (DATA_STORAGE.ballDiff.GetAngle() - world->me->GetAbsDir()).GetAbsDeg();
		const float ball_power_rate = world->params->server->kick_power_rate * (1 - 0.25f * (dir_diff/180) - 0.25f * (DATA_STORAGE.ballDistance - world->me->type->player_size - world->params->server->ball_size) / world->me->type->kickable_margin);

		float maxacc = world->params->server->maxpower * ball_power_rate;
		if(maxacc > world->params->server->ball_accel_max)
		{
			maxacc = world->params->server->ball_accel_max;
		}

// ---------------

		Vector rel_ciel = ciel - world->ball->pos;
		Vector need_vel = rel_ciel * (1 / world->params->server->ball_decay);
		
		if(fabs(need_vel.GetLength()) > (2 * world->params->server->ball_speed_max)) 
			need_vel = need_vel * (2 * world->params->server->ball_speed_max) / fabs(need_vel.GetLength());

		double need_vel_size = 1.14f * fabs(need_vel.GetLength());

		if(need_vel_size > maxacc) 
		{
			need_vel.SetX(need_vel.GetX() * (float)(maxacc / need_vel_size));
			need_vel.SetY(need_vel.GetY() * (float)(maxacc / need_vel_size));
		}

		//kvadraticka rovnica pre vypocet velkosti rychlosti kopnutia...
		double a = (need_vel.GetX() * need_vel.GetX()) + (need_vel.GetY() * need_vel.GetY());
		double b = -2 * (need_vel.GetX() * need_vel.GetX() + need_vel.GetY() * need_vel.GetY());
		double c = need_vel.GetX() * need_vel.GetX() + need_vel.GetY() * need_vel.GetY() - maxacc * maxacc;
		double D = b*b - 4*a*c;

		Vector kick_vel;

		if(D < 0)
		{
			// Nemozeme uplne zastavit loptu a kopnut do nej -> pokusime sa iba zastavit ju
			kick_vel = world->ball->vel * -1.0f;
		}
		else
		{
			float t1 = (float)((-b + sqrt(D)) / (2*a));
			float t2 = (float)((-b - sqrt(D)) / (2*a));

			if(t1 < 0.45)
				t1 = t2;
			if(t1 < 0.45)
			{
				// Nemozeme dosiahnut viac ako 0.2 nasobok pozadovanej sily -> pokusime sa najskor zastavit loptu
				kick_vel.SetX(- world->ball->vel.GetX());
				kick_vel.SetY(- world->ball->vel.GetY());
			}
			else
			{
				if(t1 > 1.)
					t1 = 1.;
				kick_vel = need_vel * t1 - world->ball->vel;
			}
		}

		float req_acc = fabs(kick_vel.GetLength());
		float kick_dir = (kick_vel.GetAngle().GetDeg() - world->me->GetAbsDir().GetDeg());



		float req_power = req_acc / ball_power_rate;
    
		if(req_power > world->params->server->maxpower)
		{
		  req_power = world->params->server->maxpower;
		}
    
		commandGenerator->Kick(req_power, kick_dir);
		
	}
}
